#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞 
@File ：testlidar.py
@Author ：ljx
@Date ：2025/8/8 下午5:23 
"""
import rospy
from sensor_msgs.msg import LaserScan
import math

def scan_callback(scan_data):
    # 打印基本信息
    print("\n--- LiDAR 数据详情 ---")
    print(f"角度范围: [{math.degrees(scan_data.angle_min):.1f}°, {math.degrees(scan_data.angle_max):.1f}°]")
    print(f"角度分辨率: {math.degrees(scan_data.angle_increment):.3f}°/点")
    print(f"有效距离范围: [{scan_data.range_min}m, {scan_data.range_max}m]")
    print(f"总数据点数: {len(scan_data.ranges)}\n")

    # 打印表头
    print("索引 | 角度(°) | 距离(m) | 有效")
    print("----------------------------------------")

    # 打印每个数据点
    for i in range(len(scan_data.ranges)):
        angle_deg = math.degrees(scan_data.angle_min + i * scan_data.angle_increment)
        distance = scan_data.ranges[i]
        is_valid = scan_data.range_min <= distance <= scan_data.range_max

        print(f"{i:4} | {angle_deg:7.2f}° | {distance:7.3f}m | {'✓' if is_valid else '✗'}")

if __name__ == "__main__":
    rospy.init_node("lidar_data_printer")
    rospy.Subscriber("/scan", LaserScan, scan_callback)
    print("等待 LiDAR 数据... (Ctrl+C 退出)")
    rospy.spin()
